#include "DC_Motor.h"

int WUCHA_MA = 0; //误差变量
int WUCHA_MB = 0; //误差变量



/*********************
 * ----初始化
*********************/
void DC_Motor::Init()
{
  pinMode(M1_Dir_Pin,OUTPUT);
  pinMode(M1_Speed_Pin,OUTPUT);
  pinMode(M2_Dir_Pin,OUTPUT);
  pinMode(M2_Speed_Pin,OUTPUT);
/*
  pinMode(8,INPUT);
  while(digitalRead(8) == 1);
  delay(1000);
  */
}
/********************
* 马达误差调试
*********************/
void DC_Motor::WuCha(String DJ1,int WUCHA)
{
  if (DJ1 == "MA")
    WUCHA_MA += WUCHA;

  else if (DJ1 == "MB")
    WUCHA_MB += WUCHA;
}

/*********************
 * 马达
*********************/
void DC_Motor::MD(String DJ,int SUDU)
{

  if (DJ == "ALL")
  {
    if ( SUDU == 0 )
    {
      digitalWrite(M1_Dir_Pin,LOW);
      digitalWrite(M2_Dir_Pin,LOW);
      analogWrite(M1_Speed_Pin,0);
      analogWrite(M2_Speed_Pin,0);
    }
    else if (SUDU > 0)
    {
      digitalWrite(M1_Dir_Pin,HIGH);
      digitalWrite(M2_Dir_Pin,HIGH);
      analogWrite(M1_Speed_Pin,255-(SUDU + WUCHA_MA));
      analogWrite(M2_Speed_Pin,255-(SUDU + WUCHA_MB));
    }
    else
    {
      digitalWrite(M1_Dir_Pin,LOW);
      digitalWrite(M2_Dir_Pin,LOW);
      analogWrite(M1_Speed_Pin,abs(SUDU - WUCHA_MA));
      analogWrite(M2_Speed_Pin,abs(SUDU - WUCHA_MB));
      
    }
  }

  else if (DJ == "MA")
  {
    if ( SUDU == 0 )
    {
      digitalWrite(M1_Dir_Pin,LOW);
      analogWrite(M1_Speed_Pin,0);
    }
    else if (SUDU > 0)
    {
      digitalWrite(M1_Dir_Pin,HIGH);
      analogWrite(M1_Speed_Pin,255-(SUDU + WUCHA_MA));
    }
    else
    {
      digitalWrite(M1_Dir_Pin,LOW);
      analogWrite(M1_Speed_Pin,abs(SUDU - WUCHA_MA));
    }
  }
  
  else if (DJ == "MB")
  {
    if ( SUDU == 0 )
    {
      digitalWrite(M2_Dir_Pin,LOW);
      analogWrite(M2_Speed_Pin,0);
    }
    else if (SUDU > 0)
    {
      digitalWrite(M2_Dir_Pin,HIGH);
      analogWrite(M2_Speed_Pin,255-(SUDU + WUCHA_MB));
    }
    else
    {
      digitalWrite(M2_Dir_Pin,LOW);
      analogWrite(M2_Speed_Pin,abs(SUDU - WUCHA_MB));
    }
  }
}
